package java_test;

import java.util.HashSet;
import java.util.LinkedList;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Set;

/**
 * Created by 默默 on 2018/3/27.
 */

public class FindPath {
    public static final char BAR = '@';

    private class Coord implements Comparable<Coord> {
        char value;
        int x;
        int y;
        Coord parent;
        int g;
        int h;
        int f;

        @Override
        public int compareTo(Coord o) {
            return f - o.f;
        }
    }

    private PriorityQueue<Coord> openMap = new PriorityQueue<>();

    private Set<Coord> closeMap = new HashSet<>();
    //函数
    private int G(Coord start, Coord now) {
        //如果斜着走，近似模拟斜边距离的比例
        if (start.x != now.x && start.y != now.y) {
            return Math.abs(start.x - now.x) * 14;
        }
        return (Math.abs(start.x - now.x) + Math.abs(start.y - now.y)) * 10;
    }
    //函数H 曼哈顿距离*10
    private int H(Coord end, Coord now) {
        return (Math.abs(end.x - now.x) + Math.abs(end.y - now.y)) * 10;
    }

    private int F(Coord start, Coord now, Coord end) {
        return G(start, now) + H(end, now);
    }

    public void findPath(char[][] map, int startX, int startY, int endX, int endY) {

        Coord[][] coords = convert(map);
        Coord startCoord = coords[startX][startY];
        Coord endCoord = coords[endX][endY];
        startCoord.g = 0;
        startCoord.h = H(endCoord, startCoord);
        startCoord.f = startCoord.g + startCoord.h;
        //1.把起始格添加到开启列表。
        openMap.add(startCoord);
        //2. 重复如下的工作：
        while (!openMap.isEmpty()) {
            //a) 寻找开启列表中F值最低的格子。我们称它为当前格。
            Coord nowCoord = openMap.poll();
            //b) 把它切换到关闭列表。
            closeMap.add(nowCoord);
            if (nowCoord.equals(endCoord)) {
                break;
            }
            //c) 对相邻的8格中的每一个
            for (Coord next : getNearByCoords(coords, nowCoord)) {
                //如果它不可通过或者已经在关闭列表中，略过它。反之如下。
                if (next.value == BAR || closeMap.contains(next)) {
                    continue;
                }
                //如果它不在开启列表中。把当前格作为这一格的父节点。记录这一格的F,G,和H值，把它添加开启列表中。
                if (!openMap.contains(next)) {
                    next.parent = nowCoord;
                    next.g = nowCoord.g + G(nowCoord, next);
                    next.h = H(endCoord, next);
                    next.f = next.g + next.h;
                    //计算值后再放入开启列表中
                    openMap.add(next);
                } else {//如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。
                    if (next.g > nowCoord.g + G(nowCoord, next)) {
                        next.parent = nowCoord;
                        next.g = nowCoord.g + G(nowCoord, next);
                        next.f = next.g + next.h;
                    }
                }
            }
        }

        if (openMap.isEmpty()) {
            System.out.println("没有找到路径");
        } else {
            printPath(endCoord);
            System.out.println();
            startCoord.value = 'S';
            endCoord.value = 'E';

            int col = map[0].length;
            int row = map.length;
            for (int i = 0; i < row; i++) {
                for (int j = 0; j < col; j++) {
                    System.out.print(coords[i][j].value + " ");
                }
                System.out.println();
            }
        }
    }

    private void printPath(Coord end) {
        if (end.parent != null) {
            end.parent.value = '#';
            printPath(end.parent);
        }
        System.out.print("(" + end.x + "," + end.y + ")-> ");
    }

    private List<Coord> getNearByCoords(Coord[][] coords, Coord nowCoord) {
        List<Coord> coordList = new LinkedList<>();
        //上
        if (nowCoord.x - 1 >= 0) {
            coordList.add(coords[nowCoord.x - 1][nowCoord.y]);
        }
        //右上
        if (nowCoord.x - 1 >= 0 && nowCoord.y + 1 < coords[0].length) {
            coordList.add(coords[nowCoord.x - 1][nowCoord.y + 1]);
        }
        //右
        if (nowCoord.y + 1 < coords[0].length) {
            coordList.add(coords[nowCoord.x][nowCoord.y + 1]);
        }
        //右下
        if (nowCoord.x + 1 < coords.length && nowCoord.y + 1 < coords[0].length) {
            coordList.add(coords[nowCoord.x + 1][nowCoord.y + 1]);
        }
        //下
        if (nowCoord.x + 1 < coords.length) {
            coordList.add(coords[nowCoord.x + 1][nowCoord.y]);
        }
        //左下
        if (nowCoord.x + 1 < coords.length && nowCoord.y - 1 >= 0) {
            coordList.add(coords[nowCoord.x + 1][nowCoord.y - 1]);
        }
        //左
        if (nowCoord.y - 1 >= 0) {
            coordList.add(coords[nowCoord.x][nowCoord.y - 1]);
        }
        //左上
        if (nowCoord.x - 1 >= 0 && nowCoord.y - 1 >= 0) {
            coordList.add(coords[nowCoord.x - 1][nowCoord.y - 1]);
        }
        return coordList;
    }

    private Coord[][] convert(char[][] map) {
        int col = map[0].length;
        int row = map.length;
        Coord[][] coords = new Coord[row][col];
        for (int i = 0; i < row; i++) {
            for (int j = 0; j < col; j++) {
                Coord coord = new Coord();
                coord.x = i;
                coord.y = j;
                coord.value = map[i][j];
                coords[i][j] = coord;
            }
        }
        return coords;
    }

    public static void main(String[] args) {
        char[][] map = {
                {'.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '@', '.', '.', '.'},
                {'.', '.', '.', '@', '.', '.', '.'},
                {'.', '.', '.', '@', '.', '.', '.'},
                {'.', '.', '.', '@', '.', '.', '.'},
                {'.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '.', '.', '.'},
//                {'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
//                {'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
//                {'.', '.', '.', '@', '@', '.', '.', '.', '.', '.', '.', '.'},
//                {'.', '.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.'},
//                {'.', '.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.'},
//                {'.', '.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.'},
//                {'.', '.', '.', '@', '@', '.', '.', '.', '.', '.', '.', '.'},
//                {'.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.', '.'},
//                {'.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.', '.'},
//                {'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
//                {'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
//                {'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
        };
        new FindPath().findPath(map, 2, 1, 2, 5);
    }

}